#ifndef Map_H_
#define Map_H_

//g2o
#include "g2o/core/graph_optimizer_sparse.h"
#include "g2o/core/hyper_graph.h"
#include "g2o/core/block_solver.h"
#include "g2o/core/solver.h"
#include "g2o/solvers/pcg/linear_solver_pcg.h"
#include "g2o/math_groups/se3quat.h"
#include "g2o/core/structure_only_solver.h"
#include "g2o/types/slam3d/vertex_se3_quat.h"
#include "g2o/types/slam3d/edge_se3_quat.h"

//other
#include <stdio.h>
#include <string.h>
#include "RGBDFrame.h"

#include "FrameMatcher.h"
#include "SurfRansacMatcherI.h"
//#include "RGBDCamera.h"
//#include "myutils.h"
#include <string>
#include <iostream>
#include <pcl/io/pcd_io.h>
#include <pcl/point_types.h>
#include <pcl/registration/icp.h>
#include <pcl/filters/passthrough.h>
#include "Transformation.h"
#include "FrameNode.h"
#include "FrameMatcher.h"
#include "SurfRansacMatcherI.h"
#include "PlanePointRansacMatcherI.h"


using namespace std;
typedef std::vector<Transformation, Eigen::aligned_allocator<Eigen::Vector4f> > TransformationVector;
typedef std::vector<FrameNode, Eigen::aligned_allocator<Eigen::Vector4f> > FrameNodeVector;

namespace cvap_6D_slam {
class Map
{
	public:
	int nr_frames;
	FrameNode * first;
	FrameNode * last;
	FrameMatcher * matcher;
	
	
//	RGBDCamera * camera;
	boost::shared_ptr<pcl::visualization::PCLVisualizer> viewer;
	
	Map();
	~Map(); 
	bool addFrame(RGBDFrame * frame);
//	void slam(RGBDCamera * camera);
//	void slam_once(RGBDCamera * camera);
    
    Transformation * getSolidTransformation(FrameNode * mid,int offset);
    
	void addTransformation(Transformation * transformation);
	void addNode(FrameNode * node);
	g2o::EdgeSE3 * getG2OEdge(Transformation * transformation);
	g2o::VertexSE3 * getG2OVertex(FrameNode * node);
	g2o::SparseOptimizer * startNewGraph();
	
	//pcl::PointCloud<pcl::PointXYZRGB>::Ptr vizualizeTransformation(Transformation * transformation);
	pcl::PointCloud<pcl::PointXYZRGB>::Ptr getMapCloud();
/*
	void store(string path);
	void load(string path);
	void setViewer(pcl::visualization::CloudViewer * viewer);
*/
	void clearGraph();
	void resetGraph();
	void optimizeGraph();
	void optimizeTransformation(RGBDCamera * camera);
	void setVisualization(boost::shared_ptr<pcl::visualization::PCLVisualizer> view);
    
    private:
	g2o::SparseOptimizer	graphoptimizer;
};
}
#endif
